/***************************************************************************************************************************
* one_drone_mission_planning.cpp
*
* Author: SFL
*
* Update Time: 2021.4.22
*
* Introduction:  无人争锋比赛的状态机节点，也就是任务节点 
*         1. 根据穿框任务设计状态机自动更新。
*         2. 从command_from_mavros.h读取无人机的状态信息（DroneState.msg）。
*         3. 不使用任何算法，直接将参考位置信息输出，利用飞控中自己的位置环算法获得飞机的状态更新
*         4. 通过command_to_mavros.h将计算出来的控制指令发送至飞控（通过mavros包）(mavros package will send the message to PX4 as Mavlink msg)
*         5. PX4 firmware will recieve the Mavlink msg by mavlink_receiver.cpp in mavlink module.
*         
***************************************************************************************************************************/


#include <ros/ros.h>
#include <vector>
#include <fstream>
#include <tf/tf.h>
#include <Eigen/Eigen>
#include "RangeImpl.h"

#include <state_from_mavros.h>
#include <command_to_mavros.h>

#include <px4_command_utils.h>
#include <path_planning_utils.h>
#include <height_controller_cascade_PID.h>
#include <LR_controller_cascade_PID.h>

// #include <wrzf_pkg/ControlCommand.h>
#include <wrzf_pkg/DroneState.h>
#include <wrzf_pkg/Topic_for_log.h>
#include "wrzf_pkg/MissionState.h"
#include "wrzf_pkg/TrajectoryPoint.h"
#include "TFLuna_i2c/RadarAvoidance.h"
#include "fsd_common_msgs/img_pro_info.h"
// #include <wrzf_pkg/UAVPositionDetectionInfo.h>
#include <serialport/ALL_UAVs_LLA.h>
#include <std_msgs/Bool.h>
#include <std_msgs/UInt8.h>

#include <global2local.h>
#include <formation_avoidance.h>

using namespace std;

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>变量声明<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
// wrzf_pkg::ControlCommand Command_Now;                      //无人机当前执行命令
// wrzf_pkg::ControlCommand Command_Last;                     //无人机上一条执行命令
// wrzf_pkg::ControlCommand Command_to_gs;                    //发送至地面站的指令
wrzf_pkg::MissionState Mission_State;                      //无人机任务状态
wrzf_pkg::TrajectoryPoint Reference_State;                 //无人机参考控制量
wrzf_pkg::DroneState _DroneState;                          //无人机状态量
mavros_msgs::HomePosition Home_Position;                   //无人机home的位置
fsd_common_msgs::img_pro_info camera_data;                         //摄像头捕捉到的框的信息
TFLuna_i2c::RadarAvoidance radar_avoidance_data;              //雷达避障数据（有判断是否有障碍物flag以及避障方向）
// wrzf_pkg::UAVPositionDetectionInfo _UAV_Detection_Info;
vector<Eigen::Vector3d> all_uavs_lla_;                    //其他无人机的经纬海拔
bool takeoff_flag = false;                                //所有无人机一键起飞的flag
bool data_radio_station_flag = true;                        //使用数传的数据来避障
bool teleradio_update = false;                              //数传的数据是否更新了
float cam_timeout = 0.0;                                      //判断摄像头是否丢失
int kill_cam = 0;


int CAM_CENTER_X = 0;                                        //摄像头视野中心坐标
int CAM_CENTER_Z = 0;
// wrzf_pkg::Topic_for_log _Topic_for_log;                  //用于日志记录的topic
Eigen::Vector3d pos_sp;
Eigen::Quaterniond quaternion_sp;
Eigen::Vector3d rpy_sp;
float thrust_sp;
float d_pos_body[2];        //the desired xy position in Body Frame
float d_pos_enu[2];         //the desired xy position in enu Frame (The origin point is the drone)
float cur_time;                                             //当前时间，也就是启控时间
float go_foward_time;                                       //前飞时间，在进入GoForward模式下飞了多久
float TotalGoForwardTime;   //前飞总时长，当前飞的时间超过这个总时长的时候就切出GoForward模式
float forward_pitch;
float Takeoff_height;                                       //起飞高度
float Disarm_height;                                        //自动上锁高度（目前看来完全没什么用）
int Flag_printf;
double ahead_distance;//实时目标点向前的距离
double k; //速度的增益
int QuadID;
//Geigraphical fence 地理围栏
Eigen::Vector2f geo_fence_x;
Eigen::Vector2f geo_fence_y;
Eigen::Vector2f geo_fence_z;
//起飞的位置
Eigen::Vector3d Takeoff_position = Eigen::Vector3d(0.0,0.0,0.0);
Eigen::Vector3d avoid_others = Eigen::Vector3d(0.0,0.0,0.0);
Eigen::Vector3d origin_lla;
Eigen::Vector3d box_1_lla;
Eigen::Vector3d box_2_lla;
float box_1_heading;
float box_2_heading; 
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>函数声明<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int check_failsafe();
// void printf_param();
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回调函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

void home_pos_cb(const mavros_msgs::HomePosition::ConstPtr& msg)
{
    Home_Position = *msg;
}

void drone_state_cb(const wrzf_pkg::DroneState::ConstPtr& msg)
{
    _DroneState = *msg;

    _DroneState.time_from_start = cur_time;

    if (Mission_State.State != wrzf_pkg::MissionState::Land)
    {
        // Check for geo fence: If drone is out of the geo fence, it will return now.
        if(check_failsafe() == 1)
        {
            Mission_State.State = wrzf_pkg::MissionState::Return;
        }
    }
}

void cam_cb(const fsd_common_msgs::img_pro_info::ConstPtr& msg)
{
    camera_data = *msg;
}

void takeoff_flag_cb(const std_msgs::Bool::ConstPtr& msg)
{
    takeoff_flag = msg->data;
}

void radar_avoidance_cb(const TFLuna_i2c::RadarAvoidance::ConstPtr& msg)
{
    radar_avoidance_data = *msg;
}

void all_uavs_lla_cb(const serialport::ALL_UAVs_LLA::ConstPtr& msg)
{
    all_uavs_lla_.clear();
    Eigen::Vector3d temp_uav_lla;
    for(auto uav_lla : msg->all_uavs_lla)
    {
        temp_uav_lla[0] = uav_lla.latitude;
        temp_uav_lla[1] = uav_lla.longitude;
        temp_uav_lla[2] = uav_lla.altitude;
        all_uavs_lla_.push_back(temp_uav_lla);
    }
    // generate_avoidance_direction(origin_lla, all_uavs_lla_, _DroneState, avoid_others, QuadID);
    teleradio_update = true;
}

// void uav_detection_info_cb(const wrzf_pkg::UAVPositionDetectionInfo::ConstPtr& msg)
// {
//     _UAV_Detection_Info = *msg;
//     if(!_UAV_Detection_Info.detected)
//     {
//         teleradio_update = false;
//     }
// }
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
    ros::init(argc, argv, "mission_planning");
    ros::NodeHandle nh("~");
 
    //【订阅】无人机当前状态
    // 本话题来自根据需求自定px4_pos_estimator.cpp
    ros::Subscriber drone_state_sub = nh.subscribe<wrzf_pkg::DroneState>("/px4_command/drone_state", 5, drone_state_cb);
    //【发布】无人机log发布到地面站
    // 发布log消息至ground_station.cpp
    // ros::Publisher log_pub = nh.advertise<wrzf_pkg::Topic_for_log>("/px4_command/topic_for_log", 10);
    //【订阅】home的位置
    ros::Subscriber home_position_sub = nh.subscribe<mavros_msgs::HomePosition>("/mavros/home_position/home", 10, home_pos_cb);
    //【订阅】摄像头获取到的框在像素坐标系下的位置
    ros::Subscriber cam_sub = nh.subscribe<fsd_common_msgs::img_pro_info>("/img_pro_info", 1, cam_cb);
    //【订阅】起飞flag
    ros::Subscriber takeoff_flag_sub = nh.subscribe<std_msgs::Bool>("/px4_command/takeoff_flag", 10, takeoff_flag_cb);
    //【订阅】所有无人机的经纬海拔
    ros::Subscriber all_uavs_lla_sub = nh.subscribe<serialport::ALL_UAVs_LLA>("/all_uavs_position/global", 1, all_uavs_lla_cb);
    //【订阅】雷达避障动作数据
    ros::Subscriber radar_avoidance_sub = nh.subscribe<TFLuna_i2c::RadarAvoidance>("/radar_avoidance_data", 1, radar_avoidance_cb);
    //【订阅】识别到的前面最近的无人机的位置[相机系下：右方x为正，下方y为正，前方z为正]
    // ros::Subscriber uav_detection_info_sub = nh.subscribe<wrzf_pkg::UAVPositionDetectionInfo>("/forward_uav_position/camera", 1, uav_detection_info_cb);
    //【发布】无人机的起飞顺序
    ros::Publisher takeoff_order_pub = nh.advertise<std_msgs::UInt8>("/takeoff_order",10);

    // 参数读取
    nh.param<float>("Takeoff_height", Takeoff_height, 1.0);
    nh.param<float>("Disarm_height", Disarm_height, 0.15);
    nh.param<int>("Flag_printf", Flag_printf, 0);
    
    nh.param<float>("geo_fence/x_min", geo_fence_x[0], -10.0);
    nh.param<float>("geo_fence/x_max", geo_fence_x[1], 10.0);
    nh.param<float>("geo_fence/y_min", geo_fence_y[0], -10.0);
    nh.param<float>("geo_fence/y_max", geo_fence_y[1], 10.0);
    nh.param<float>("geo_fence/z_min", geo_fence_z[0], -10.0);
    nh.param<float>("geo_fence/z_max", geo_fence_z[1], 10.0);
    
    //默认的经纬海拔给的都是哈尔滨的粗略地理位置，参数表中是实际比赛时框的位置。
    nh.param<double>("box_1_latitude", box_1_lla[0], 45.00);
    nh.param<double>("box_1_longitude", box_1_lla[1], 126.00);
    nh.param<double>("box_1_altitude", box_1_lla[2], 127.95);
    nh.param<float>("box_1_heading", box_1_heading, 0.0);
    nh.param<double>("box_2_latitude", box_2_lla[0], 45.01);
    nh.param<double>("box_2_longitude", box_2_lla[1], 126.01);
    nh.param<double>("box_2_altitude", box_2_lla[2], 127.90);
    nh.param<float>("box_2_heading", box_2_heading, 0);

    nh.param<float>("forward_pitch", forward_pitch, 0.2);
    nh.param<float>("TotalGoForwardTime", TotalGoForwardTime, 10.0);
    nh.param<double>("ahead_distance", ahead_distance, 1.0);
    nh.param<double>("k", k, 0.1);
    nh.param<int>("QuadID", QuadID,0);
    

    // 位置控制选取为50Hz，主要取决于位置状态的更新频率
    ros::Rate rate(50);
    
    // 先读取一些飞控的数据
    for(int i=0;i<50;i++)
    {
        ros::spinOnce();
        rate.sleep();
    }

    while(ros::ok() && _DroneState.gps_status == -1)
    {
        ros::spinOnce();
        rate.sleep();
        ROS_INFO("Waiting for GPS...");
    }
    ROS_INFO("GPS fixed");

    
    // 用于与mavros通讯的类，通过mavros发送控制指令至飞控【本程序->mavros->飞控】
    command_to_mavros _command_to_mavros;

    height_controller_cascade_PID height_controller_cascade_pid;
    LR_controller_cascade_PID lr_controller_cascade_pid;

    //*************************************路径规划****************************************
    // vector<double> x = {0.0, 2.0, 3.0, 4.0, 5.0, 4.5, 3.5, 2.0, 0.0};//规划路径的关键航点x
    // vector<double> y = {0.0, 0.5, 0.0, -0.5, 0.0, 1.5, 2.5, 2.0, 5.0};//规划路径的关键航点y
    //vector<double> x = {0.0, 50.000, 153.003, 203.003, 253.003, 353.003, 453.003, 553.003, 653.003, 703.003, 753.003, 752.999, 702.999, 652.999, 552.999, 452.999, 352.999, 252.999, 202.999, 152.999};//规划路径的关键航点x
    //vector<double> y = {0.0, 50.9527, 50.9527, 50.9527, 50.9527, 50.9527, 50.9527, 50.9527, 50.9527, 50.9527, 50.9527, 111.228, 111.228, 111.228, 111.228, 111.228, 111.228, 111.228, 111.228, 111.228};//规划路径的关键航点y
    // vector<double> x = {0.0, 153.003/30, 203.003/30, 253.003/30, 353.003/30, 453.003/30, 553.003/30, 653.003/30, 703.003/30, 753.003/30, 752.999/30, 702.999/30, 652.999/30, 552.999/30, 452.999/30, 352.999/30, 252.999/30, 202.999/30, 152.999/30};//规划路径的关键航点x
    // vector<double> y = {0.0, 50.9527/10, 50.9527/10, 50.9527/10, 50.9527/10, 50.9527/10, 50.9527/10, 50.9527/10, 50.9527/10, 50.9527/10, 111.228/10, 111.228/10, 111.228/10, 111.228/10, 111.228/10, 111.228/10, 111.228/10, 111.228/10, 111.228/10};//规划路径的关键航点y
    
    vector<double> x ;//规划路径的关键航点x
    vector<double> y ;//规划路径的关键航点y
    x.push_back(0.0);
    y.push_back(0.0);

    origin_lla = Eigen::Vector3d(Home_Position.geo.latitude,Home_Position.geo.longitude,0.0);
    create_local_path_key_point1(origin_lla, box_1_lla, box_1_heading, x, y);
    create_local_path_key_point2(origin_lla, box_2_lla, box_2_heading, x, y);

    //*******************************************************************************
    // cout << "x: ";
    // for (int i = 0; i < x.size(); i++)
    // {
    //     // x[i] = x[i]/30;
    //     cout << x[i] << " ";
    // }
    // cout << endl;
    // cout << "y: ";
    // for (int i = 0; i < x.size(); i++)
    // {
    //     // y[i] = y[i]/10;
    //     cout << y[i] << " ";
    // }
    // cout << endl;
    // cout << "origin lla: " << origin_lla << endl;
    // cout << "box_1_lla: " << box_1_lla << endl;
    // cout << "box_2_lla: " << box_2_lla << endl;
    // cout << "box_2_heading" << box_2_heading << endl;

    vector<double> tx;  //x坐标
    vector<double> ty;  //y坐标
    vector<double> tyaw;//偏航
    vector<double> tc;  //曲率
    int target_index;          //目标索引

    Spline2D csp = generate_target_course(x, y, tx, ty, tyaw, tc);
    // cout << tx.size() << endl;
    // int check_flag;
    // cin >> check_flag;
    //************************************************************************************
    std_msgs::UInt8 takeoff_order;
    takeoff_order.data = 0b00000000;
    // _UAV_Detection_Info.detected = false;

    // Set the takeoff position
    Takeoff_position[0] = _DroneState.position[0];
    Takeoff_position[1] = _DroneState.position[1];
    Takeoff_position[2] = _DroneState.position[2];
    // Takeoff_position[0] = Home_Position.position.x;
    // Takeoff_position[1] = Home_Position.position.y;
    // Takeoff_position[2] = Home_Position.position.z;

    // 初始化命令-
    // 默认设置：Idle模式 电机怠速旋转 等待来自上层的控制指令
    Mission_State.State = wrzf_pkg::MissionState::Initial;
    Reference_State.Sub_mode  = command_to_mavros::XYZ_POS;
    Reference_State.position_ref[0] = 0;
    Reference_State.position_ref[1] = 0;
    Reference_State.position_ref[2] = 0;
    Reference_State.velocity_ref[0] = 0;
    Reference_State.velocity_ref[1] = 0;
    Reference_State.velocity_ref[2] = 0;
    Reference_State.acceleration_ref[0] = 0;
    Reference_State.acceleration_ref[1] = 0;
    Reference_State.acceleration_ref[2] = 0;
    Reference_State.yaw_ref = 0;

    //中间变量
    float radio_link_disconnect_time = 0.0;
    bool break_for = false;
    vector<Eigen::Vector3d> all_uavs_lla_last = all_uavs_lla_;
    fsd_common_msgs::img_pro_info camera_data_last = camera_data;
    // 记录启控时间
    ros::Time begin_time = ros::Time::now();
    float last_time = px4_command_utils::get_time_in_sec(begin_time);
    float dt = 0;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主  循  环<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
    while (ros::ok())
    {
        // 当前时间
        cur_time = px4_command_utils::get_time_in_sec(begin_time);
        dt = cur_time  - last_time;
        dt = constrain_function2(dt, 0.01, 0.03);
        last_time = cur_time;
        
        //执行回调函数
        ros::spinOnce();

        // ////////////////////////模拟GPS信号丢失, 只有仿真的时候使用，记得在实际测试时候删掉////////////////////////////////////////////////////
        // if (sqrt(pow(_DroneState.position[0]-400, 2) + pow(_DroneState.position[1]-50, 2) + pow(_DroneState.position[2]-10, 2))  < 200 ||
        //     sqrt(pow(_DroneState.position[0]-400, 2) + pow(_DroneState.position[1]-100, 2) + pow(_DroneState.position[2]-10, 2)) < 200)
        // {
        //     _DroneState.gps_status = -1;
        // }
        // else
        // {
        //     _DroneState.gps_status = 1;
        // }
        // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

        //判断数传的数据是否有效
        for (int i = 0; i < all_uavs_lla_.size(); i++)
        {
            if(all_uavs_lla_.at(i) == all_uavs_lla_last.at(i))//直接让两个Vector3d类型相等不知道是否可以
            {   //只要有一架一样，就加入计时,计时一次就退出for循环
                radio_link_disconnect_time += dt;
                break_for = true;
                break;
            }
            else//这一段逻辑是不缜密的，正常应该是所有数据都不一样才是false
            {
                break_for = false;
            }
        }
        if(!break_for)//只要没退出for就说明所有无人机状态都更新了，说明数传在正常工作
        {
            radio_link_disconnect_time = 0.0; //计时清零
            data_radio_station_flag = true;
        }
        
        if (radio_link_disconnect_time > 2.0)//说明有2秒钟的时间数传的数据都没有变化了
        {
            data_radio_station_flag = false;
        }

        if (camera_data.header.seq == camera_data_last.header.seq)
        {
            cam_timeout += dt;
        }
        else
        {
            cam_timeout = 0.0;
            kill_cam = 0;
        }
        if (cam_timeout > 1.0 && kill_cam==0)
        {
            system("rosnode kill /usb_cam");
            kill_cam = kill_cam + 1;
        }
        if (cam_timeout > 3.0)
        {
            if (_DroneState.gps_status >= 0)
            {
                _command_to_mavros.returnhome();
            }
            else
            {
                thrust_sp = 0.6;
                rpy_sp[0] = 0.0;
                rpy_sp[1] = 0.0;
                rpy_sp[2] = 0.0;
                quaternion_sp = quaternion_from_rpy(rpy_sp);
                _command_to_mavros.send_attitude_setpoint(quaternion_sp,thrust_sp);  
            }
        }
        
        
        all_uavs_lla_last = all_uavs_lla_;
        camera_data_last = camera_data;

        //测试起飞flag
        // cout << "起飞flag： "<< takeoff_flag << endl;
        // cout << "第1架高度：" << all_uavs_lla_.at(0)[2] << endl;
        // cout << "第2架高度：" << all_uavs_lla_.at(1)[2] << endl;
        // cout << "第3架高度：" << all_uavs_lla_.at(2)[2] << endl;
        // cout << "第4架高度：" << all_uavs_lla_.at(3)[2] << endl;
        //测试起飞顺序
        // cout << "起飞顺序为：";
        // if (takeoff_flag)
        // {
	    // int temp = takeoff_order.data;
        // cout << temp;
        // }
        // cout << endl;
        //测试是否收到其他飞机位置
        // cout << "所有无人机经纬度：" << endl;
        // cout << "无人机数量：" << all_uavs_lla_.size() << endl;
        // for (int i = 0; i < all_uavs_lla_.size(); i++)
        // {
        //     cout << "第" << i << "架无人机： " << endl; 
        //     cout << all_uavs_lla_.at(i) << endl;
        // }
        
        //状态机开启
        switch (Mission_State.State)
        {
            // 【Initial】 初始化状态，负责检测GPS state，切入offboard和解锁
            case wrzf_pkg::MissionState::Initial:
                _command_to_mavros.idle();
                Reference_State.yaw_ref = _DroneState.attitude[2];
                //数传主节点所在的飞机判断起飞顺序
                if(QuadID == all_uavs_lla_.size() - 1)//最后一架是数传主节点所在的无人机，如果主节点在其他号码上就换一下
                {
                    for (int i = 0; i < all_uavs_lla_.size(); i++)
                    {
                        if (all_uavs_lla_.at(i)[2] > 10)//如果i飞机的高度超过10m
                        {
                            takeoff_order.data |= (1 << (i+1));
                            // cout << "第" << i << "架高度超过2m，高度为：" << all_uavs_lla_.at(i)[2] << endl;
                        }
                    }
                    takeoff_order_pub.publish(takeoff_order);
                }
                //小电脑与飞控连接且gps搜星正常，切offboard
                if(_DroneState.connected && _DroneState.gps_status >= 0)
                {
                    _command_to_mavros.offboard();
                }
                if(_DroneState.connected && _DroneState.gps_status >= 0 && _DroneState.mode == "OFFBOARD" && takeoff_flag)//&&takeoff_flag
                {
                    _command_to_mavros.arm();
                }
                if(_DroneState.connected && _DroneState.gps_status >= 0 && _DroneState.armed && _DroneState.mode == "OFFBOARD" && takeoff_flag)//&&takeoff_flag
                {
                    Mission_State.State = wrzf_pkg::MissionState::Takeoff;
                }
                break;
            // 【Takeoff】 从home位置原地起飞至指定高度，偏航角也保持当前角度
            case wrzf_pkg::MissionState::Takeoff:

                Reference_State.Sub_mode  = command_to_mavros::XYZ_POS;
                Reference_State.position_ref[0] = Takeoff_position[0];
                Reference_State.position_ref[1] = Takeoff_position[1];
                Reference_State.position_ref[2] = Takeoff_position[2] + Takeoff_height;
                Reference_State.velocity_ref[0] = 0;
                Reference_State.velocity_ref[1] = 0;
                Reference_State.velocity_ref[2] = 0;
                Reference_State.acceleration_ref[0] = 0;
                Reference_State.acceleration_ref[1] = 0;
                Reference_State.acceleration_ref[2] = 0;
                Reference_State.yaw_ref = _DroneState.attitude[2]; //rad

                pos_sp[0] = Reference_State.position_ref[0];
                pos_sp[1] = Reference_State.position_ref[1];
                pos_sp[2] = Reference_State.position_ref[2];

                _command_to_mavros.send_pos_setpoint(pos_sp, Reference_State.yaw_ref);

                if(abs(_DroneState.position[2] - Reference_State.position_ref[2]) < 2)
                {
                    Mission_State.State = wrzf_pkg::MissionState::PathPatrol;
                }
                break;
            // 【PathPatrol】 跟随特定路径,三次平滑轨迹。
            case wrzf_pkg::MissionState::PathPatrol:
                if (_DroneState.gps_status == -1) // GPS STATUS NO FIX 
                {
                    // if (_UAV_Detection_Info.detected)
                    // {
                    //     Reference_State.position_ref[2] = _DroneState.position[2] - _UAV_Detection_Info.position[1];
                    //     rpy_sp[0] = 0.05 * _UAV_Detection_Info.position[0];
                    //     rpy_sp[1] = 0.4 - 1 / (_UAV_Detection_Info.position[2] - 0.3775) + 0.2;
                    // }
                    // else
                    // {
                    //     rpy_sp[0] = 0;
                    //     rpy_sp[1] = 0.4;
                    // }

                    rpy_sp[0] = 0;
                    rpy_sp[1] = forward_pitch;
                    
                    thrust_sp = height_controller_cascade_pid.pos_controller(_DroneState, Reference_State, dt);

                    rpy_sp[2] = Reference_State.yaw_ref;
                   
                    quaternion_sp = quaternion_from_rpy(rpy_sp);
                   
                    _command_to_mavros.send_attitude_setpoint(quaternion_sp,thrust_sp);  
                }
                else // GPS STATUS FIX
                {
                    target_index = calc_target_index(_DroneState, tx, ty,ahead_distance,k);
                    Reference_State.position_ref[0] = tx[target_index];
                    Reference_State.position_ref[1] = ty[target_index];
                    Reference_State.yaw_ref = tyaw[target_index];
                    if (abs(yaw_ref_norm(tyaw[target_index] - box_1_heading)) < M_PI/3)
                    {
                        Reference_State.yaw_ref = box_1_heading;
                    }
                    if (abs(yaw_ref_norm(tyaw[target_index] - box_2_heading)) < M_PI/3)
                    {
                        Reference_State.yaw_ref = box_2_heading;
                    }
                    
                    geometry_msgs::Point end_point, waypoint;
                    end_point.x = x.back();
                    end_point.y = y.back();
                    end_point.z = Reference_State.position_ref[2];

                    if(target_index == tx.size() - 1)
                    {
                        waypoint = waypoint_trajectory(_DroneState, end_point);
                        Reference_State.position_ref[0] = waypoint.x;
                        Reference_State.position_ref[1] = waypoint.y;
                    }

                    
                    //队内避障
                    if (data_radio_station_flag)
                    {
                        if (teleradio_update)
                        {
                            generate_avoidance_direction(origin_lla, all_uavs_lla_, _DroneState, avoid_others, QuadID);
                        }
                        // else if(!teleradio_update && _UAV_Detection_Info.detected)
                        // {
                        //     generate_avoidance_direction_by_vision(_UAV_Detection_Info, _DroneState, avoid_others);
                        // }
                        else
                        {
                            avoid_others = Eigen::Vector3d(0.0,0.0,0.0);
                        }
                        
                    }
                    else 
                    {
                        // if (_UAV_Detection_Info.detected)
                        // {
                        //     generate_avoidance_direction_by_vision(_UAV_Detection_Info, _DroneState, avoid_others);
                        // }
                        // else
                        // {
                            avoid_others = Eigen::Vector3d(0.0,0.0,0.0);
                        // }
                    }

                    Reference_State.position_ref[0] = Reference_State.position_ref[0] + avoid_others[0];
                    Reference_State.position_ref[1] = Reference_State.position_ref[1] + avoid_others[1];
                    Reference_State.position_ref[2] = Reference_State.position_ref[2] + avoid_others[2];
                    
                    pos_sp[0] = Reference_State.position_ref[0];
                    pos_sp[1] = Reference_State.position_ref[1];
                    pos_sp[2] = Reference_State.position_ref[2];

                    _command_to_mavros.send_pos_setpoint(pos_sp, Reference_State.yaw_ref);
                }

                if (camera_data.find_box_flag)
                {
                    Mission_State.State = wrzf_pkg::MissionState::TrackBox;
                }
                
                if (abs(x[x.size()-1] - _DroneState.position[0]) + abs(y[y.size()-1] - _DroneState.position[1]) < 1.0)
                {
                    Mission_State.State = wrzf_pkg::MissionState::Return;
                }

                if (radar_avoidance_data.avoidance_flag == true)
                {
                    Mission_State.State = wrzf_pkg::MissionState::Avoidance;
                }
                
                break;

            // 【TrackBox】 跟踪红框
            case wrzf_pkg::MissionState::TrackBox:
                // _DroneState.gps_status = -1; 
                if (_DroneState.gps_status == -1) // GPS STATUS NO FIX 
                {
                    Reference_State.position_ref[2] = _DroneState.position[2] + 0.01 * (camera_data.y_pos - CAM_CENTER_Z);

                    if (abs(camera_data.height_difference) <= 0)
                    {
                        Reference_State.yaw_ref = _DroneState.attitude[2];
                    }
                    else
                    {
                        Reference_State.yaw_ref = _DroneState.attitude[2] - 0.002 * (camera_data.height_difference);
                    }
                    
                    // Reference_State.yaw_ref = _DroneState.attitude[2] - 0.08 * (camera_data.height_difference / (camera_data.left_edge + camera_data.right_edge+0.0000001));

                    Reference_State.yaw_ref = yaw_ref_norm(Reference_State.yaw_ref);
                    
                    // if (_UAV_Detection_Info.detected)
                    // {
                    //     rpy_sp[1] = 0.4 - 1 / (_UAV_Detection_Info.position[2] - 0.3775) + 0.2;
                    // }
                    // else
                    // {
                        rpy_sp[1] = forward_pitch;
                    // }
                    
                    thrust_sp = height_controller_cascade_pid.pos_controller(_DroneState, Reference_State, dt);

                    rpy_sp[0] = lr_controller_cascade_pid.pos_controller(CAM_CENTER_X - camera_data.x_pos, _DroneState, dt);
                    rpy_sp[2] = Reference_State.yaw_ref;
                   
                    quaternion_sp = quaternion_from_rpy(rpy_sp);
                   
                    _command_to_mavros.send_attitude_setpoint(quaternion_sp,thrust_sp);  
                }
                else // GPS STATUS FIX
                {
                    d_pos_body[0] = ahead_distance;
                    d_pos_body[1] = - 0.02 * (camera_data.x_pos - CAM_CENTER_X);
                    d_pos_body[1] = constrain_function(d_pos_body[1], 1.0);
                    px4_command_utils::rotation_yaw(_DroneState.attitude[2], d_pos_body, d_pos_enu);

                    Reference_State.position_ref[0] = _DroneState.position[0] + d_pos_enu[0];
                    Reference_State.position_ref[1] = _DroneState.position[1] + d_pos_enu[1];
                    Reference_State.velocity_ref[0] = 0;
                    Reference_State.velocity_ref[1] = 0;
                    Reference_State.position_ref[2] = _DroneState.position[2] + 0.01 * (camera_data.y_pos - CAM_CENTER_Z);
                    Reference_State.velocity_ref[2] = 0; 
                    if (abs(camera_data.height_difference) <= 0)
                    {
                        Reference_State.yaw_ref = _DroneState.attitude[2];
                    }
                    else
                    {
                        Reference_State.yaw_ref = _DroneState.attitude[2] - 0.002 * (camera_data.height_difference);
                    }
                    Reference_State.yaw_ref = yaw_ref_norm(Reference_State.yaw_ref);

                    //队内避障
                    if (data_radio_station_flag)
                    {
                        if (teleradio_update)
                        {
                            generate_avoidance_direction(origin_lla, all_uavs_lla_, _DroneState, avoid_others, QuadID);
                        }
                        // else if(!teleradio_update && _UAV_Detection_Info.detected)
                        // {
                        //     generate_avoidance_direction_by_vision(_UAV_Detection_Info, _DroneState, avoid_others);
                        // }
                        else
                        {
                            avoid_others = Eigen::Vector3d(0.0,0.0,0.0);
                        }
                        
                    }
                    else 
                    {
                        // if (_UAV_Detection_Info.detected)
                        // {
                        //     generate_avoidance_direction_by_vision(_UAV_Detection_Info, _DroneState, avoid_others);
                        // }
                        // else
                        // {
                            avoid_others = Eigen::Vector3d(0.0,0.0,0.0);
                        // }
                    }

                    Reference_State.position_ref[0] = Reference_State.position_ref[0] + avoid_others[0];
                    Reference_State.position_ref[1] = Reference_State.position_ref[1] + avoid_others[1];
                    Reference_State.position_ref[2] = Reference_State.position_ref[2] + avoid_others[2];

                    pos_sp[0] = Reference_State.position_ref[0];
                    pos_sp[1] = Reference_State.position_ref[1];
                    pos_sp[2] = Reference_State.position_ref[2];

                    _command_to_mavros.send_pos_setpoint(pos_sp, Reference_State.yaw_ref);
                }
                
                if (!camera_data.find_box_flag)
                {
                    Mission_State.State = wrzf_pkg::MissionState::GoForward;
                }
                
                break;

            // 【GoForward】 距离框太近之后会看不见红框，要开环往前飞一段距离
            case wrzf_pkg::MissionState::GoForward:
                go_foward_time += dt;
                if (_DroneState.gps_status == -1) // GPS STATUS NO FIX 
                {
                    // if (_UAV_Detection_Info.detected)
                    // {
                    //     Reference_State.position_ref[2] = _DroneState.position[2] - _UAV_Detection_Info.position[1];
                    //     rpy_sp[0] = 0.05 * _UAV_Detection_Info.position[0];
                    //     rpy_sp[1] = 0.4 - 1 / (_UAV_Detection_Info.position[2] - 0.3775) + 0.2;
                    // }
                    // else
                    // {
                        rpy_sp[0] = 0;
                        rpy_sp[1] = forward_pitch;
                    // }

                    thrust_sp = height_controller_cascade_pid.pos_controller(_DroneState, Reference_State, dt);
                    // rpy_sp[0] = 0;
                    // rpy_sp[1] = 0.4;
                    rpy_sp[2] = Reference_State.yaw_ref;
                   
                    quaternion_sp = quaternion_from_rpy(rpy_sp);
                   
                    _command_to_mavros.send_attitude_setpoint(quaternion_sp,thrust_sp);
                }
                else // GPS STATUS FIX 
                {
                    d_pos_body[0] = ahead_distance;
                    d_pos_body[1] = 0.0;
                    px4_command_utils::rotation_yaw(_DroneState.attitude[2], d_pos_body, d_pos_enu);

                    Reference_State.position_ref[0] = _DroneState.position[0] + d_pos_enu[0];
                    Reference_State.position_ref[1] = _DroneState.position[1] + d_pos_enu[1];
                    Reference_State.velocity_ref[0] = 0;
                    Reference_State.velocity_ref[1] = 0;
                    Reference_State.velocity_ref[2] = 0; 
                    // Reference_State.yaw_ref = _DroneState.attitude[2] + Reference_State.yaw_ref;//

                    //队内避障
                    if (data_radio_station_flag)
                    {
                        if (teleradio_update)
                        {
                            generate_avoidance_direction(origin_lla, all_uavs_lla_, _DroneState, avoid_others, QuadID);
                        }
                        // else if(!teleradio_update && _UAV_Detection_Info.detected)
                        // {
                        //     if (_UAV_Detection_Info.position[2] < 5 && _UAV_Detection_Info.position[2] >= 2)
                        //     {
                        //         d_pos_body[0] = -2 * (1 / (_UAV_Detection_Info.position[2] - (2 - 0.2)) - 
                        //                         1 / (5 - (2 - 0.2)));
                        //     }
                        //     else if(_UAV_Detection_Info.position[2] < 2)
                        //     {
                        //         d_pos_body[0] = -10;
                        //     }
                        //     else
                        //     {
                        //         d_pos_body[0] = 0;
                        //     }
                        //     // d_pos_body[1] = -_UAV_Detection_Info.position[0];
                        //     d_pos_body[1] = 0;
                        //     px4_command_utils::rotation_yaw(_DroneState.attitude[2], d_pos_body, d_pos_enu);
                        //     //前进方向避障，侧向跟随，高度不变
                        //     avoid_others[0] = d_pos_enu[0];
                        //     avoid_others[1] = d_pos_enu[1];
                        //     avoid_others[2] = 0;
                        // }
                        else
                        {
                            avoid_others = Eigen::Vector3d(0.0,0.0,0.0);
                        }
                        
                    }
                    else 
                    {
                        // if (_UAV_Detection_Info.detected)
                        // {
                        //     if (_UAV_Detection_Info.position[2] < 5 && _UAV_Detection_Info.position[2] >= 2)
                        //     {
                        //         d_pos_body[0] = -2 * (1 / (_UAV_Detection_Info.position[2] - (2 - 0.2)) - 
                        //                         1 / (5 - (2 - 0.2)));
                        //     }
                        //     else if(_UAV_Detection_Info.position[2] < 2)
                        //     {
                        //         d_pos_body[0] = -10;
                        //     }
                        //     else
                        //     {
                        //         d_pos_body[0] = 0;
                        //     }
                        //     // d_pos_body[1] = -_UAV_Detection_Info.position[0];
                        //     d_pos_body[1] = 0;
                        //     px4_command_utils::rotation_yaw(_DroneState.attitude[2], d_pos_body, d_pos_enu);
                        //     //前进方向避障，侧向跟随，高度不变
                        //     avoid_others[0] = d_pos_enu[0];
                        //     avoid_others[1] = d_pos_enu[1];
                        //     avoid_others[2] = 0;
                        // }
                        // else
                        // {
                            avoid_others = Eigen::Vector3d(0.0,0.0,0.0);
                        // }
                    }
                    Reference_State.position_ref[0] = Reference_State.position_ref[0] + avoid_others[0];
                    Reference_State.position_ref[1] = Reference_State.position_ref[1] + avoid_others[1];
                    Reference_State.position_ref[2] = Reference_State.position_ref[2] + avoid_others[2];

                    pos_sp[0] = Reference_State.position_ref[0];
                    pos_sp[1] = Reference_State.position_ref[1];
                    pos_sp[2] = Reference_State.position_ref[2];

                    _command_to_mavros.send_pos_setpoint(pos_sp, Reference_State.yaw_ref);
                }
                
                
                if (go_foward_time > TotalGoForwardTime)
                {
                    Mission_State.State = wrzf_pkg::MissionState::PathPatrol;
                    go_foward_time = 0.0;
                }
                if (camera_data.find_box_flag)
                {
                    Mission_State.State = wrzf_pkg::MissionState::TrackBox;
                    go_foward_time = 0.0;
                }
                if (radar_avoidance_data.avoidance_flag)
                {
                    Mission_State.State = wrzf_pkg::MissionState::Avoidance;
                    go_foward_time = 0.0;
                }
                break;

            // 【Avoidance】 毫米波雷达或者激光雷达避障
            case wrzf_pkg::MissionState::Avoidance:
                if (_DroneState.gps_status == -1)
                {
                    switch (radar_avoidance_data.direction)
                    {
                        case TFLuna_i2c::RadarAvoidance::Up:
                            Reference_State.position_ref[2] = _DroneState.position[2] + 0.5;
                            break;
                        case TFLuna_i2c::RadarAvoidance::Down:
                            Reference_State.position_ref[2] = _DroneState.position[2] - 0.5;
                            break;
                        case TFLuna_i2c::RadarAvoidance::Left:
                            rpy_sp[0] = -0.2;
                            rpy_sp[1] = 0.0;
                            break;
                        case TFLuna_i2c::RadarAvoidance::Right:
                            rpy_sp[0] = 0.2;
                            rpy_sp[1] = 0.0;
                            break;
                        case TFLuna_i2c::RadarAvoidance::Hold:
                            rpy_sp[0] = 0.0;
                            rpy_sp[1] = 0.0;
                            break;
                        case TFLuna_i2c::RadarAvoidance::Forward:
                            /* 尚未开发 */
                            break;
                        case TFLuna_i2c::RadarAvoidance::Back:
                            /* 尚未开发 */
                            break;
                    }
                    thrust_sp = height_controller_cascade_pid.pos_controller(_DroneState, Reference_State, dt);
                    
                    rpy_sp[2] = Reference_State.yaw_ref;

                    quaternion_sp = quaternion_from_rpy(rpy_sp);
                   
                    _command_to_mavros.send_attitude_setpoint(quaternion_sp,thrust_sp);
                }
                else
                {
                    switch (radar_avoidance_data.direction)
                    {
                        case TFLuna_i2c::RadarAvoidance::Up:
                            Reference_State.position_ref[2] = _DroneState.position[2] + 0.5;
                            break;
                        case TFLuna_i2c::RadarAvoidance::Down:
                            Reference_State.position_ref[2] = _DroneState.position[2] - 0.5;
                            break;
                        case TFLuna_i2c::RadarAvoidance::Left:
                            d_pos_body[0] = 0.0;
                            d_pos_body[1] = -0.5;
                            px4_command_utils::rotation_yaw(_DroneState.attitude[2], d_pos_body, d_pos_enu);
                            Reference_State.position_ref[0] = _DroneState.position[0] + d_pos_enu[0];
                            Reference_State.position_ref[1] = _DroneState.position[1] + d_pos_enu[1];
                            break;
                        case TFLuna_i2c::RadarAvoidance::Right:
                            d_pos_body[0] = 0.0;
                            d_pos_body[1] = 0.5;
                            px4_command_utils::rotation_yaw(_DroneState.attitude[2], d_pos_body, d_pos_enu);
                            Reference_State.position_ref[0] = _DroneState.position[0] + d_pos_enu[0];
                            Reference_State.position_ref[1] = _DroneState.position[1] + d_pos_enu[1];
                            break;
                        case TFLuna_i2c::RadarAvoidance::Hold:
                            /* 不需要写代码就是hold */
                            break;
                        case TFLuna_i2c::RadarAvoidance::Forward:
                            /* code for condition */
                            break;
                        case TFLuna_i2c::RadarAvoidance::Back:
                            /* code for condition */
                            break;
                    }
                    pos_sp[0] = Reference_State.position_ref[0];
                    pos_sp[1] = Reference_State.position_ref[1];
                    pos_sp[2] = Reference_State.position_ref[2];

                    _command_to_mavros.send_pos_setpoint(pos_sp, Reference_State.yaw_ref);
                }
                

                if (radar_avoidance_data.avoidance_flag == false)
                {
                    Mission_State.State = wrzf_pkg::MissionState::GoForward;
                }
                break;

            // 【Land】 降落。当前位置原地降落,此时已经切出offboard
            case wrzf_pkg::MissionState::Land:
                _command_to_mavros.land();
                Takeoff_position[0] = Home_Position.position.x;
                Takeoff_position[1] = Home_Position.position.y;
                Takeoff_position[2] = Home_Position.position.z;
                break;

            // 【Return】 返航，维持当前高度飞回到home点
            case wrzf_pkg::MissionState::Return:
            	 _command_to_mavros.returnhome();
                //geometry_msgs::Point waypoint = waypoint_trajectory(_DroneState, Home_Position.position);
                //Reference_State.position_ref[0] = waypoint.x;
                //Reference_State.position_ref[1] = waypoint.y;
                

                //队内避障
                //if (data_radio_station_flag)
                //{
                //    if (teleradio_update)
                //    {
                //        generate_avoidance_direction(origin_lla, all_uavs_lla_, _DroneState, avoid_others, QuadID);
                //    }
                    // else if(!teleradio_update && _UAV_Detection_Info.detected)
                    // {
                    //     generate_avoidance_direction_by_vision(_UAV_Detection_Info, _DroneState, avoid_others);
                    // }
                //    else
                //    {
                //        avoid_others = Eigen::Vector3d(0.0,0.0,0.0);
                //    }
                    // generate_avoidance_direction(origin_lla, all_uavs_lla_, _DroneState, avoid_others, QuadID);
                //}
                //else 
                //{
                    // if (_UAV_Detection_Info.detected)
                    // {
                    //     generate_avoidance_direction_by_vision(_UAV_Detection_Info, _DroneState, avoid_others);
                    // }
                    // else
                    // {
                //        avoid_others = Eigen::Vector3d(0.0,0.0,0.0);
                    // }
                //}

                //Reference_State.position_ref[0] = Reference_State.position_ref[0] + avoid_others[0];
                //Reference_State.position_ref[1] = Reference_State.position_ref[1] + avoid_others[1];
                //Reference_State.position_ref[2] = Reference_State.position_ref[2] + avoid_others[2];

                //pos_sp[0] = Reference_State.position_ref[0];
                //pos_sp[1] = Reference_State.position_ref[1];
                //pos_sp[2] = Reference_State.position_ref[2];

                //_command_to_mavros.send_pos_setpoint(pos_sp, Reference_State.yaw_ref);

                //if(abs(Home_Position.position.x - _DroneState.position[0]) + abs(Home_Position.position.y - _DroneState.position[1]) < 1.0)
                //{
                //    Mission_State.State = wrzf_pkg::MissionState::Land;
                //}
                break;
            }
        
        
        if(Flag_printf == 1)
        {
            //打印无人机状态
            px4_command_utils::prinft_drone_state(_DroneState);
            // 打印上层控制指令
            px4_command_utils::printf_mission_reference(Mission_State,Reference_State);

        }else if(((int)(cur_time*10) % 50) == 0)
        {
            cout << "px4_pos_controller is running for :" << cur_time << " [s] "<<endl;
        }

        rate.sleep();
    }

    return 0;
}

// void printf_param()
// {
//     cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>> Parameter <<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
//     cout << "Takeoff_height: "<< Takeoff_height<<" [m] "<<endl;
//     cout << "Disarm_height : "<< Disarm_height <<" [m] "<<endl;
//     cout << "geo_fence_x : "<< geo_fence_x[0] << " [m]  to  "<<geo_fence_x[1] << " [m]"<< endl;
//     cout << "geo_fence_y : "<< geo_fence_y[0] << " [m]  to  "<<geo_fence_y[1] << " [m]"<< endl;
//     cout << "geo_fence_z : "<< geo_fence_z[0] << " [m]  to  "<<geo_fence_z[1] << " [m]"<< endl;
    
// }

int check_failsafe()
{
    if (_DroneState.position[0] < geo_fence_x[0] || _DroneState.position[0] > geo_fence_x[1] ||
        _DroneState.position[1] < geo_fence_y[0] || _DroneState.position[1] > geo_fence_y[1] ||
        _DroneState.position[2] < geo_fence_z[0] || _DroneState.position[2] > geo_fence_z[1])
    {
        return 1;
        cout << "Out of the geo fence, the drone is returning... "<< endl;
    }
    else{
        return 0;
    }
}
